#include "xict_cpp/utils/util.h"

namespace xict_cpp {
    namespace utils {
        Eigen::Matrix3d rotationVectorToMatrix(const cv::Vec3d& rvec) {
            cv::Mat R;
            cv::Rodrigues(rvec, R);
            Eigen::Matrix3d rotation_matrix;
            for (int i = 0; i < 3; ++i) {
                for (int j = 0; j < 3; ++j) {
                    rotation_matrix(i, j) = R.at<double>(i, j);
                }
            }
            return rotation_matrix;
        }

        std::vector<cv::Point3d> GenerateBezierCurve(const cv::Point3d& p0,
                                                     const cv::Point3d& p1,
                                                     const cv::Point3d& p2) {
            std::vector<cv::Point3d> curve;
            int num_points = 1000;   // 曲线点数

            for (int i = 0; i < num_points; ++i) {
                double t = static_cast<double>(i) / (num_points - 1);
                double x = (1 - t) * (1 - t) * p0.x + 2 * (1 - t) * t * p1.x +
                           t * t * p2.x;
                double y = (1 - t) * (1 - t) * p0.y + 2 * (1 - t) * t * p1.y +
                           t * t * p2.y;
                double z = (1 - t) * (1 - t) * p0.z + 2 * (1 - t) * t * p1.z +
                           t * t * p2.z;
                curve.emplace_back(x, y, z);
            }

            return curve;
        }
    }   // namespace utils
}   // namespace xict_cpp